function [solzarray,xarray]= provideFamilySolution_matrixnormold(F,w,h,numparams,numsols,Kguess,type)
if(numparams~=2)
    error('only works with 2');
end

if(exist('type','var')~=1)
    type='foc1';
end

if(size(F,1)==0)
    solzarray=[];
    xarray=[];
    
    return ;
end


if(strcmpi(type, 'foc1')==1)
    [solzarray,xarray]=findFocal11(F,w,h,numsols,Kguess);
elseif(strcmpi(type, 'foc2')==1)
    [solzarray,xarray]=findFocal12(F,w,h,numsols,Kguess);
elseif(strcmpi(type, 'xc')==1)
    [solzarray,xarray]=findOCxc(F,w,h,numsols,Kguess);
elseif(strcmpi(type, 'yc')==1)
    [solzarray,xarray]=findOCyc(F,w,h,numsols,Kguess);
else
    error('no such option');
end

end



function [solzarray,xfarray]=findFocal11(F,w,h,numsols,Kguess)


F=F/norm(F,'fro');

focal2start=max(100,(-w)+Kguess(2,2));
focal2end=min(2*w ,(w)+Kguess(2,2));

if(numsols==1)
    xarray=[Kguess(2,2)];
else
    step=((focal2end-focal2start)/(numsols-1));
    solzarray=zeros(numsols,1);
    xarray=focal2start:step:focal2end;
    xfarray=zeros(numsols,1);
end

f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);


xc=Kguess(1,3);
yc=Kguess(2,3);



minans=1;

for i=1:numsols
    
    foc2=xarray(i);
    p=[f1^4
        0
        (4*f1^2*(f1^2*xc^2 + 2*f1*f2*xc*yc + 2*f1*f3*xc + f2^2*foc2^2 + f2^2*yc^2 + 2*f2*f3*yc + f3^2) - 2*f1^2*((f3 + f1*xc + f2*yc)^2 + (f7 + f1*xc + f4*yc)^2 + f2^2*foc2^2 + f4^2*foc2^2) + 4*f1^2*(f7 + f1*xc + f4*yc)^2 + 4*f1^2*f4^2*foc2^2)
        0
        (2*(f7 + f1*xc + f4*yc)^4 - 2*f1^2*(foc2^2*(f6 + f4*xc + f5*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f5^2*foc2^4) + 2*(f1^2*xc^2 + 2*f1*f2*xc*yc + 2*f1*f3*xc + f2^2*foc2^2 + f2^2*yc^2 + 2*f2*f3*yc + f3^2)^2 - ((f3 + f1*xc + f2*yc)^2 + (f7 + f1*xc + f4*yc)^2 + f2^2*foc2^2 + f4^2*foc2^2)^2 + 2*f4^4*foc2^4 + 8*f1*(f2*(f8 + f2*xc + f5*yc)*foc2^2 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc))*(f7 + f1*xc + f4*yc) + 4*f4^2*foc2^2*(f7 + f1*xc + f4*yc)^2 + 8*f1*f4*foc2^2*(f3*f6 + f1*f4*xc^2 + f2*f5*yc^2 + f1*f6*xc + f3*f4*xc + f2*f6*yc + f3*f5*yc + f2*f5*foc2^2 + f1*f5*xc*yc + f2*f4*xc*yc))
        0
        (- 2*((f3 + f1*xc + f2*yc)^2 + (f7 + f1*xc + f4*yc)^2 + f2^2*foc2^2 + f4^2*foc2^2)*(foc2^2*(f6 + f4*xc + f5*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f5^2*foc2^4) + 4*(f2*(f8 + f2*xc + f5*yc)*foc2^2 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc))^2 + 4*foc2^2*(f3*f6 + f1*f4*xc^2 + f2*f5*yc^2 + f1*f6*xc + f3*f4*xc + f2*f6*yc + f3*f5*yc + f2*f5*foc2^2 + f1*f5*xc*yc + f2*f4*xc*yc)^2 + 4*(foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)*(f7 + f1*xc + f4*yc)^2 + 4*f4^2*foc2^4*(f4^2*xc^2 + 2*f4*f5*xc*yc + 2*f4*f6*xc + f5^2*foc2^2 + f5^2*yc^2 + 2*f5*f6*yc + f6^2) + 8*f4*foc2*(f5*(f8 + f2*xc + f5*yc)*foc2^3 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc2)*(f7 + f1*xc + f4*yc))
        0
        (4*(f5*(f8 + f2*xc + f5*yc)*foc2^3 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc2)^2 - (foc2^2*(f6 + f4*xc + f5*yc)^2 + foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f5^2*foc2^4)^2 + 2*(foc2^2*(f8 + f2*xc + f5*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)^2 + 2*foc2^4*(f4^2*xc^2 + 2*f4*f5*xc*yc + 2*f4*f6*xc + f5^2*foc2^2 + f5^2*yc^2 + 2*f5*f6*yc + f6^2)^2)];
    r = roots(p');
    
    answ=processRoots_focalonly(r);
    if(answ>minans)
        solzarray(i,1)=answ;
    elseif(i>1)
        solzarray(i,1)=solzarray(i-1,1);
    else
        solzarray(i,1)=0;
    end
    xfarray(i,1)= foc2;
end


end


function [solzarray,xfarray]=findFocal12(F,w,h,numsols,Kguess)


F=F/norm(F,'fro');

focal1start=max(100,(-w)+Kguess(1,1));
focal1end=min(2*w ,(w)+Kguess(2,2));

if(numsols==1)
    xarray=[Kguess(1,1)];
else
    step=((focal1end-focal1start)/(numsols-1));
    solzarray=zeros(numsols,1);
    xarray=focal1start:step:focal1end;
    xfarray=zeros(numsols,1);
end



f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);


xc=Kguess(1,3);
yc=Kguess(2,3);


minans=1;


for i=1:numsols
    
    foc1=xarray(i);
    p=[
        f5^4
        0
        (4*f5^2*(f4^2*foc1^2 + f4^2*xc^2 + 2*f4*f5*xc*yc + 2*f4*f6*xc + f5^2*yc^2 + 2*f5*f6*yc + f6^2) - 2*f5^2*((f6 + f4*xc + f5*yc)^2 + (f8 + f2*xc + f5*yc)^2 + f2^2*foc1^2 + f4^2*foc1^2) + 4*f5^2*(f8 + f2*xc + f5*yc)^2 + 4*f2^2*f5^2*foc1^2)
        0
        (2*(f8 + f2*xc + f5*yc)^4 - 2*f5^2*(foc1^2*(f3 + f1*xc + f2*yc)^2 + foc1^2*(f7 + f1*xc + f4*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f1^2*foc1^4) + 2*(f4^2*foc1^2 + f4^2*xc^2 + 2*f4*f5*xc*yc + 2*f4*f6*xc + f5^2*yc^2 + 2*f5*f6*yc + f6^2)^2 - ((f6 + f4*xc + f5*yc)^2 + (f8 + f2*xc + f5*yc)^2 + f2^2*foc1^2 + f4^2*foc1^2)^2 + 2*f2^4*foc1^4 + 8*f5*(f4*(f7 + f1*xc + f4*yc)*foc1^2 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc))*(f8 + f2*xc + f5*yc) + 4*f2^2*foc1^2*(f8 + f2*xc + f5*yc)^2 + 8*f2*f5*foc1^2*(f3*f6 + f1*f4*xc^2 + f2*f5*yc^2 + f1*f6*xc + f3*f4*xc + f2*f6*yc + f3*f5*yc + f1*f4*foc1^2 + f1*f5*xc*yc + f2*f4*xc*yc))
        0
        (- 2*((f6 + f4*xc + f5*yc)^2 + (f8 + f2*xc + f5*yc)^2 + f2^2*foc1^2 + f4^2*foc1^2)*(foc1^2*(f3 + f1*xc + f2*yc)^2 + foc1^2*(f7 + f1*xc + f4*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f1^2*foc1^4) + 4*(f4*(f7 + f1*xc + f4*yc)*foc1^2 + (f6 + f4*xc + f5*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc))^2 + 4*foc1^2*(f3*f6 + f1*f4*xc^2 + f2*f5*yc^2 + f1*f6*xc + f3*f4*xc + f2*f6*yc + f3*f5*yc + f1*f4*foc1^2 + f1*f5*xc*yc + f2*f4*xc*yc)^2 + 4*(foc1^2*(f7 + f1*xc + f4*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)*(f8 + f2*xc + f5*yc)^2 + 4*f2^2*foc1^4*(f1^2*foc1^2 + f1^2*xc^2 + 2*f1*f2*xc*yc + 2*f1*f3*xc + f2^2*yc^2 + 2*f2*f3*yc + f3^2) + 8*f2*foc1*(f1*(f7 + f1*xc + f4*yc)*foc1^3 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc1)*(f8 + f2*xc + f5*yc))
        0
        (4*(f1*(f7 + f1*xc + f4*yc)*foc1^3 + (f3 + f1*xc + f2*yc)*(f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)*foc1)^2 - (foc1^2*(f3 + f1*xc + f2*yc)^2 + foc1^2*(f7 + f1*xc + f4*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2 + f1^2*foc1^4)^2 + 2*(foc1^2*(f7 + f1*xc + f4*yc)^2 + (f9 + f3*xc + f7*xc + f6*yc + f8*yc + f1*xc^2 + f5*yc^2 + f2*xc*yc + f4*xc*yc)^2)^2 + 2*foc1^4*(f1^2*foc1^2 + f1^2*xc^2 + 2*f1*f2*xc*yc + 2*f1*f3*xc + f2^2*yc^2 + 2*f2*f3*yc + f3^2)^2)
        ];
    r = roots(p');
    
       answ=processRoots_focalonly(r);
    if(answ>minans)
        solzarray(i,1)=answ;
    elseif(i>1)
        solzarray(i,1)=solzarray(i-1,1);
    else
        solzarray(i,1)=0;
    end
    xfarray(i,1)= foc1;
end


end
function [solzarray,xfarray]=findOCxc(F,w,h,numsols,Kguess)

F=F/norm(F,'fro');



xcstart=max(20,(-w/4)+Kguess(1,3));
xcend=min(w/2+w/4 ,(w/4)+Kguess(1,3));

if(numsols==1)
    xarray=[Kguess(1,3)];
else
    step=((xcend-xcstart)/(numsols-1));
    solzarray=zeros(numsols,1);
    xarray=xcstart:step:xcend;
    xfarray=zeros(numsols,1);
end




minans=1;

f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);

foc1=Kguess(1,1);
foc2=Kguess(2,2);






for i=1:numsols
    
    yc=xarray(i);
    p=[f1^4
        
    (4*f1^3*(f3 + f7 + f2*yc + f4*yc))
    
    (4*f1^2*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + f1^2*foc1^2 + f2^2*foc2^2) + 4*f1^2*(f3 + f7 + f2*yc + f4*yc)^2 + 4*f1^4*foc1^2 - 2*f1^2*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1^2*foc1^2 + f2^2*foc2^2 + f4^2*foc2^2) + 4*f1^2*f4^2*foc2^2)
    
    (4*f1^2*(2*f1*(f7 + f4*yc)*foc1^2 + 2*f2*(f8 + f5*yc)*foc2^2 + 2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)) - 2*f1^2*(2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1*foc1^2*(f3 + f2*yc) + 2*f1*foc1^2*(f7 + f4*yc) + 2*f2*foc2^2*(f8 + f5*yc) + 2*f4*foc2^2*(f6 + f5*yc)) - 4*f1*(f3 + f7 + f2*yc + f4*yc)*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1^2*foc1^2 + f2^2*foc2^2 + f4^2*foc2^2) + 8*f1*(f3 + f7 + f2*yc + f4*yc)*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + f1^2*foc1^2 + f2^2*foc2^2) + 8*f1^2*foc1^2*(f1*(f3 + f7 + f2*yc + f4*yc) + f1*(f3 + f2*yc)) + 8*f1*f4*foc2^2*(f4*(f3 + f7 + f2*yc + f4*yc) + f1*(f6 + f5*yc)))
    
    (4*foc1^2*(f1*(f3 + f7 + f2*yc + f4*yc) + f1*(f3 + f2*yc))^2 + 4*foc2^2*(f4*(f3 + f7 + f2*yc + f4*yc) + f1*(f6 + f5*yc))^2 - 2*f1^2*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f3 + f2*yc)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f6 + f5*yc)^2 + foc2^2*(f8 + f5*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) - ((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1^2*foc1^2 + f2^2*foc2^2 + f4^2*foc2^2)^2 + 2*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + f1^2*foc1^2 + f2^2*foc2^2)^2 + 4*f1^2*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f8 + f5*yc)^2) + 2*f1^4*foc1^4 + 2*f4^4*foc2^4 + 8*f1^2*foc1*(foc1*((f3 + f2*yc)*(f3 + f7 + f2*yc + f4*yc) + f1*(f9 + f6*yc + f8*yc + f5*yc^2)) + f1^2*foc1^3 + f2^2*foc1*foc2^2) + 8*f1*(2*f1*(f7 + f4*yc)*foc1^2 + 2*f2*(f8 + f5*yc)*foc2^2 + 2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2))*(f3 + f7 + f2*yc + f4*yc) - 4*f1*(f3 + f7 + f2*yc + f4*yc)*(2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1*foc1^2*(f3 + f2*yc) + 2*f1*foc1^2*(f7 + f4*yc) + 2*f2*foc2^2*(f8 + f5*yc) + 2*f4*foc2^2*(f6 + f5*yc)) + 8*f1*f4*foc2*(f1*f4*foc1^2*foc2 + f2*f5*foc2^3 + ((f6 + f5*yc)*(f3 + f7 + f2*yc + f4*yc) + f4*(f9 + f6*yc + f8*yc + f5*yc^2))*foc2) + 4*f1^2*f4^2*foc1^2*foc2^2)
    
    (4*(2*f1*(f7 + f4*yc)*foc1^2 + 2*f2*(f8 + f5*yc)*foc2^2 + 2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2))*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + f1^2*foc1^2 + f2^2*foc2^2) - 2*(2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1*foc1^2*(f3 + f2*yc) + 2*f1*foc1^2*(f7 + f4*yc) + 2*f2*foc2^2*(f8 + f5*yc) + 2*f4*foc2^2*(f6 + f5*yc))*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1^2*foc1^2 + f2^2*foc2^2 + f4^2*foc2^2) + 4*f1^2*foc1^4*(2*f1*f3 + 2*f1*f2*yc) + 4*f4^2*foc2^4*(2*f4*f6 + 2*f4*f5*yc) + 8*f1^2*foc1*(f1*(f7 + f4*yc)*foc1^3 + f2*(f8 + f5*yc)*foc1*foc2^2 + (f3 + f2*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc1) + 8*f1*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f8 + f5*yc)^2)*(f3 + f7 + f2*yc + f4*yc) - 4*f1*(f3 + f7 + f2*yc + f4*yc)*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f3 + f2*yc)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f6 + f5*yc)^2 + foc2^2*(f8 + f5*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 8*foc1*(f1*(f3 + f7 + f2*yc + f4*yc) + f1*(f3 + f2*yc))*(foc1*((f3 + f2*yc)*(f3 + f7 + f2*yc + f4*yc) + f1*(f9 + f6*yc + f8*yc + f5*yc^2)) + f1^2*foc1^3 + f2^2*foc1*foc2^2) + 8*foc2*(f4*(f3 + f7 + f2*yc + f4*yc) + f1*(f6 + f5*yc))*(f1*f4*foc1^2*foc2 + f2*f5*foc2^3 + ((f6 + f5*yc)*(f3 + f7 + f2*yc + f4*yc) + f4*(f9 + f6*yc + f8*yc + f5*yc^2))*foc2) + 8*f1*f4*foc2*(f4*(f7 + f4*yc)*foc1^2*foc2 + f5*(f8 + f5*yc)*foc2^3 + (f6 + f5*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc2) + 8*f1*f4*foc1^2*foc2^2*(f1*f6 + f3*f4 + f1*f5*yc + f2*f4*yc))
    
    (- 2*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1^2*foc1^2 + f2^2*foc2^2 + f4^2*foc2^2)*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f3 + f2*yc)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f6 + f5*yc)^2 + foc2^2*(f8 + f5*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 2*(2*f1*(f7 + f4*yc)*foc1^2 + 2*f2*(f8 + f5*yc)*foc2^2 + 2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2))^2 + 4*(foc1*((f3 + f2*yc)*(f3 + f7 + f2*yc + f4*yc) + f1*(f9 + f6*yc + f8*yc + f5*yc^2)) + f1^2*foc1^3 + f2^2*foc1*foc2^2)^2 + 2*foc1^4*((2*f1*f3 + 2*f1*f2*yc)^2 + 2*f1^2*(f1^2*foc1^2 + f2^2*foc2^2 + f2^2*yc^2 + 2*f2*f3*yc + f3^2)) + 2*foc2^4*((2*f4*f6 + 2*f4*f5*yc)^2 + 2*f4^2*(f4^2*foc1^2 + f5^2*foc2^2 + f5^2*yc^2 + 2*f5*f6*yc + f6^2)) + 4*(f1*f4*foc1^2*foc2 + f2*f5*foc2^3 + ((f6 + f5*yc)*(f3 + f7 + f2*yc + f4*yc) + f4*(f9 + f6*yc + f8*yc + f5*yc^2))*foc2)^2 - (2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1*foc1^2*(f3 + f2*yc) + 2*f1*foc1^2*(f7 + f4*yc) + 2*f2*foc2^2*(f8 + f5*yc) + 2*f4*foc2^2*(f6 + f5*yc))^2 + 4*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f8 + f5*yc)^2)*((f3 + f7 + f2*yc + f4*yc)^2 + 2*f1*(f9 + f6*yc + f8*yc + f5*yc^2) + f1^2*foc1^2 + f2^2*foc2^2) + 4*foc1^2*foc2^2*((f1*f6 + f3*f4 + f1*f5*yc + f2*f4*yc)^2 + 2*f1*f4*(f3*f6 + f2*f5*yc^2 + f2*f6*yc + f3*f5*yc + f1*f4*foc1^2 + f2*f5*foc2^2)) + 8*foc1*(f1*(f3 + f7 + f2*yc + f4*yc) + f1*(f3 + f2*yc))*(f1*(f7 + f4*yc)*foc1^3 + f2*(f8 + f5*yc)*foc1*foc2^2 + (f3 + f2*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc1) + 8*foc2*(f4*(f3 + f7 + f2*yc + f4*yc) + f1*(f6 + f5*yc))*(f4*(f7 + f4*yc)*foc1^2*foc2 + f5*(f8 + f5*yc)*foc2^3 + (f6 + f5*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc2))
    
    (8*(foc1*((f3 + f2*yc)*(f3 + f7 + f2*yc + f4*yc) + f1*(f9 + f6*yc + f8*yc + f5*yc^2)) + f1^2*foc1^3 + f2^2*foc1*foc2^2)*(f1*(f7 + f4*yc)*foc1^3 + f2*(f8 + f5*yc)*foc1*foc2^2 + (f3 + f2*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc1) + 8*(f1*f4*foc1^2*foc2 + f2*f5*foc2^3 + ((f6 + f5*yc)*(f3 + f7 + f2*yc + f4*yc) + f4*(f9 + f6*yc + f8*yc + f5*yc^2))*foc2)*(f4*(f7 + f4*yc)*foc1^2*foc2 + f5*(f8 + f5*yc)*foc2^3 + (f6 + f5*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc2) + 4*(2*f1*(f7 + f4*yc)*foc1^2 + 2*f2*(f8 + f5*yc)*foc2^2 + 2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2))*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f8 + f5*yc)^2) - 2*(2*(f3 + f7 + f2*yc + f4*yc)*(f9 + f6*yc + f8*yc + f5*yc^2) + 2*f1*foc1^2*(f3 + f2*yc) + 2*f1*foc1^2*(f7 + f4*yc) + 2*f2*foc2^2*(f8 + f5*yc) + 2*f4*foc2^2*(f6 + f5*yc))*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f3 + f2*yc)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f6 + f5*yc)^2 + foc2^2*(f8 + f5*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 4*foc1^4*(2*f1*f3 + 2*f1*f2*yc)*(f1^2*foc1^2 + f2^2*foc2^2 + f2^2*yc^2 + 2*f2*f3*yc + f3^2) + 4*foc2^4*(2*f4*f6 + 2*f4*f5*yc)*(f4^2*foc1^2 + f5^2*foc2^2 + f5^2*yc^2 + 2*f5*f6*yc + f6^2) + 8*foc1^2*foc2^2*(f1*f6 + f3*f4 + f1*f5*yc + f2*f4*yc)*(f3*f6 + f2*f5*yc^2 + f2*f6*yc + f3*f5*yc + f1*f4*foc1^2 + f2*f5*foc2^2))
    
    (2*((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f8 + f5*yc)^2)^2 + 2*foc1^4*(f1^2*foc1^2 + f2^2*foc2^2 + f2^2*yc^2 + 2*f2*f3*yc + f3^2)^2 + 2*foc2^4*(f4^2*foc1^2 + f5^2*foc2^2 + f5^2*yc^2 + 2*f5*f6*yc + f6^2)^2 - ((f9 + f6*yc + f8*yc + f5*yc^2)^2 + foc1^2*(f3 + f2*yc)^2 + foc1^2*(f7 + f4*yc)^2 + foc2^2*(f6 + f5*yc)^2 + foc2^2*(f8 + f5*yc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2)^2 + 4*(f1*(f7 + f4*yc)*foc1^3 + f2*(f8 + f5*yc)*foc1*foc2^2 + (f3 + f2*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc1)^2 + 4*(f4*(f7 + f4*yc)*foc1^2*foc2 + f5*(f8 + f5*yc)*foc2^3 + (f6 + f5*yc)*(f9 + f6*yc + f8*yc + f5*yc^2)*foc2)^2 + 4*foc1^2*foc2^2*(f3*f6 + f2*f5*yc^2 + f2*f6*yc + f3*f5*yc + f1*f4*foc1^2 + f2*f5*foc2^2)^2)];
r = roots(p');
    answ=processRoots_focalonly(r);
    if(answ>minans)
        solzarray(i,1)=answ;
    elseif(i>1)
        solzarray(i,1)=solzarray(i-1,1);
    else
        solzarray(i,1)=0;
    end
    xfarray(i,1)= yc;
end


end

function [solzarray,xfarray]=findOCyc(F,w,h,numsols,Kguess)

F=F/norm(F,'fro');

ycstart=max(20,(-w/4)+Kguess(2,3));
ycend=min(w/2+w/4 ,(w/4)+Kguess(1,3));

if(numsols==1)
    xarray=[Kguess(2,3)];
else
    step=((ycend-ycstart)/(numsols-1));
    solzarray=zeros(numsols,1);
    xfarray=zeros(numsols,1);
    xarray=ycstart:step:ycend;
end




minans=1;


f1=F(1,1);
f2=F(1,2);
f3=F(1,3);

f4=F(2,1);
f5=F(2,2);
f6=F(2,3);

f7=F(3,1);
f8=F(3,2);
f9=F(3,3);


foc1=Kguess(1,1);
foc2=Kguess(2,2);


for i=1:numsols
    
    xc=xarray(i);
    p=[f5^4
        
    (4*f5^3*(f6 + f8 + f2*xc + f4*xc))
    
    (4*f5^2*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f4^2*foc1^2 + f5^2*foc2^2) + 4*f5^2*(f6 + f8 + f2*xc + f4*xc)^2 + 4*f5^4*foc2^2 - 2*f5^2*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f2^2*foc1^2 + f4^2*foc1^2 + 2*f5^2*foc2^2) + 4*f2^2*f5^2*foc1^2)
    
    (4*f5^2*(2*f4*(f7 + f1*xc)*foc1^2 + 2*f5*(f8 + f2*xc)*foc2^2 + 2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)) - 2*f5^2*(2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2) + 2*f2*foc1^2*(f3 + f1*xc) + 2*f4*foc1^2*(f7 + f1*xc) + 2*f5*foc2^2*(f6 + f4*xc) + 2*f5*foc2^2*(f8 + f2*xc)) - 4*f5*(f6 + f8 + f2*xc + f4*xc)*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f2^2*foc1^2 + f4^2*foc1^2 + 2*f5^2*foc2^2) + 8*f5*(f6 + f8 + f2*xc + f4*xc)*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f4^2*foc1^2 + f5^2*foc2^2) + 8*f5^2*foc2^2*(f5*(f6 + f8 + f2*xc + f4*xc) + f5*(f6 + f4*xc)) + 8*f2*f5*foc1^2*(f2*(f6 + f8 + f2*xc + f4*xc) + f5*(f3 + f1*xc)))
    
    (4*foc1^2*(f2*(f6 + f8 + f2*xc + f4*xc) + f5*(f3 + f1*xc))^2 + 4*foc2^2*(f5*(f6 + f8 + f2*xc + f4*xc) + f5*(f6 + f4*xc))^2 - 2*f5^2*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f3 + f1*xc)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f6 + f4*xc)^2 + foc2^2*(f8 + f2*xc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) - ((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f2^2*foc1^2 + f4^2*foc1^2 + 2*f5^2*foc2^2)^2 + 2*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f4^2*foc1^2 + f5^2*foc2^2)^2 + 4*f5^2*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f8 + f2*xc)^2) + 2*f2^4*foc1^4 + 2*f5^4*foc2^4 + 8*f5*(2*f4*(f7 + f1*xc)*foc1^2 + 2*f5*(f8 + f2*xc)*foc2^2 + 2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2))*(f6 + f8 + f2*xc + f4*xc) - 4*f5*(f6 + f8 + f2*xc + f4*xc)*(2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2) + 2*f2*foc1^2*(f3 + f1*xc) + 2*f4*foc1^2*(f7 + f1*xc) + 2*f5*foc2^2*(f6 + f4*xc) + 2*f5*foc2^2*(f8 + f2*xc)) + 8*f5^2*foc2*(foc2*((f6 + f4*xc)*(f6 + f8 + f2*xc + f4*xc) + f5*(f9 + f3*xc + f7*xc + f1*xc^2)) + f5^2*foc2^3 + f4^2*foc1^2*foc2) + 8*f2*f5*foc1*(f1*f4*foc1^3 + f2*f5*foc1*foc2^2 + ((f3 + f1*xc)*(f6 + f8 + f2*xc + f4*xc) + f2*(f9 + f3*xc + f7*xc + f1*xc^2))*foc1) + 4*f2^2*f5^2*foc1^2*foc2^2)
    
    (4*(2*f4*(f7 + f1*xc)*foc1^2 + 2*f5*(f8 + f2*xc)*foc2^2 + 2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2))*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f4^2*foc1^2 + f5^2*foc2^2) - 2*(2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2) + 2*f2*foc1^2*(f3 + f1*xc) + 2*f4*foc1^2*(f7 + f1*xc) + 2*f5*foc2^2*(f6 + f4*xc) + 2*f5*foc2^2*(f8 + f2*xc))*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f2^2*foc1^2 + f4^2*foc1^2 + 2*f5^2*foc2^2) + 8*f5^2*foc2*(f4*(f7 + f1*xc)*foc1^2*foc2 + f5*(f8 + f2*xc)*foc2^3 + (f6 + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc2) + 4*f2^2*foc1^4*(2*f2*f3 + 2*f1*f2*xc) + 4*f5^2*foc2^4*(2*f5*f6 + 2*f4*f5*xc) + 8*f5*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f8 + f2*xc)^2)*(f6 + f8 + f2*xc + f4*xc) - 4*f5*(f6 + f8 + f2*xc + f4*xc)*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f3 + f1*xc)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f6 + f4*xc)^2 + foc2^2*(f8 + f2*xc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 8*foc2*(f5*(f6 + f8 + f2*xc + f4*xc) + f5*(f6 + f4*xc))*(foc2*((f6 + f4*xc)*(f6 + f8 + f2*xc + f4*xc) + f5*(f9 + f3*xc + f7*xc + f1*xc^2)) + f5^2*foc2^3 + f4^2*foc1^2*foc2) + 8*foc1*(f2*(f6 + f8 + f2*xc + f4*xc) + f5*(f3 + f1*xc))*(f1*f4*foc1^3 + f2*f5*foc1*foc2^2 + ((f3 + f1*xc)*(f6 + f8 + f2*xc + f4*xc) + f2*(f9 + f3*xc + f7*xc + f1*xc^2))*foc1) + 8*f2*f5*foc1*(f1*(f7 + f1*xc)*foc1^3 + f2*(f8 + f2*xc)*foc1*foc2^2 + (f3 + f1*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc1) + 8*f2*f5*foc1^2*foc2^2*(f2*f6 + f3*f5 + f1*f5*xc + f2*f4*xc))
    
    (- 2*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f2^2*foc1^2 + f4^2*foc1^2 + 2*f5^2*foc2^2)*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f3 + f1*xc)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f6 + f4*xc)^2 + foc2^2*(f8 + f2*xc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 2*(2*f4*(f7 + f1*xc)*foc1^2 + 2*f5*(f8 + f2*xc)*foc2^2 + 2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2))^2 + 4*(foc2*((f6 + f4*xc)*(f6 + f8 + f2*xc + f4*xc) + f5*(f9 + f3*xc + f7*xc + f1*xc^2)) + f5^2*foc2^3 + f4^2*foc1^2*foc2)^2 + 2*foc1^4*((2*f2*f3 + 2*f1*f2*xc)^2 + 2*f2^2*(f1^2*foc1^2 + f1^2*xc^2 + 2*f1*f3*xc + f2^2*foc2^2 + f3^2)) + 2*foc2^4*((2*f5*f6 + 2*f4*f5*xc)^2 + 2*f5^2*(f4^2*foc1^2 + f4^2*xc^2 + 2*f4*f6*xc + f5^2*foc2^2 + f6^2)) + 4*(f1*f4*foc1^3 + f2*f5*foc1*foc2^2 + ((f3 + f1*xc)*(f6 + f8 + f2*xc + f4*xc) + f2*(f9 + f3*xc + f7*xc + f1*xc^2))*foc1)^2 - (2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2) + 2*f2*foc1^2*(f3 + f1*xc) + 2*f4*foc1^2*(f7 + f1*xc) + 2*f5*foc2^2*(f6 + f4*xc) + 2*f5*foc2^2*(f8 + f2*xc))^2 + 4*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f8 + f2*xc)^2)*((f6 + f8 + f2*xc + f4*xc)^2 + 2*f5*(f9 + f3*xc + f7*xc + f1*xc^2) + f4^2*foc1^2 + f5^2*foc2^2) + 4*foc1^2*foc2^2*((f2*f6 + f3*f5 + f1*f5*xc + f2*f4*xc)^2 + 2*f2*f5*(f3*f6 + f1*f4*xc^2 + f1*f6*xc + f3*f4*xc + f1*f4*foc1^2 + f2*f5*foc2^2)) + 8*foc1*(f2*(f6 + f8 + f2*xc + f4*xc) + f5*(f3 + f1*xc))*(f1*(f7 + f1*xc)*foc1^3 + f2*(f8 + f2*xc)*foc1*foc2^2 + (f3 + f1*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc1) + 8*foc2*(f5*(f6 + f8 + f2*xc + f4*xc) + f5*(f6 + f4*xc))*(f4*(f7 + f1*xc)*foc1^2*foc2 + f5*(f8 + f2*xc)*foc2^3 + (f6 + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc2))
    
    (8*(foc2*((f6 + f4*xc)*(f6 + f8 + f2*xc + f4*xc) + f5*(f9 + f3*xc + f7*xc + f1*xc^2)) + f5^2*foc2^3 + f4^2*foc1^2*foc2)*(f4*(f7 + f1*xc)*foc1^2*foc2 + f5*(f8 + f2*xc)*foc2^3 + (f6 + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc2) + 8*(f1*f4*foc1^3 + f2*f5*foc1*foc2^2 + ((f3 + f1*xc)*(f6 + f8 + f2*xc + f4*xc) + f2*(f9 + f3*xc + f7*xc + f1*xc^2))*foc1)*(f1*(f7 + f1*xc)*foc1^3 + f2*(f8 + f2*xc)*foc1*foc2^2 + (f3 + f1*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc1) + 4*(2*f4*(f7 + f1*xc)*foc1^2 + 2*f5*(f8 + f2*xc)*foc2^2 + 2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2))*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f8 + f2*xc)^2) - 2*(2*(f6 + f8 + f2*xc + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2) + 2*f2*foc1^2*(f3 + f1*xc) + 2*f4*foc1^2*(f7 + f1*xc) + 2*f5*foc2^2*(f6 + f4*xc) + 2*f5*foc2^2*(f8 + f2*xc))*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f3 + f1*xc)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f6 + f4*xc)^2 + foc2^2*(f8 + f2*xc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2) + 4*foc1^4*(2*f2*f3 + 2*f1*f2*xc)*(f1^2*foc1^2 + f1^2*xc^2 + 2*f1*f3*xc + f2^2*foc2^2 + f3^2) + 4*foc2^4*(2*f5*f6 + 2*f4*f5*xc)*(f4^2*foc1^2 + f4^2*xc^2 + 2*f4*f6*xc + f5^2*foc2^2 + f6^2) + 8*foc1^2*foc2^2*(f2*f6 + f3*f5 + f1*f5*xc + f2*f4*xc)*(f3*f6 + f1*f4*xc^2 + f1*f6*xc + f3*f4*xc + f1*f4*foc1^2 + f2*f5*foc2^2))
    
    (2*((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f8 + f2*xc)^2)^2 + 2*foc1^4*(f1^2*foc1^2 + f1^2*xc^2 + 2*f1*f3*xc + f2^2*foc2^2 + f3^2)^2 + 2*foc2^4*(f4^2*foc1^2 + f4^2*xc^2 + 2*f4*f6*xc + f5^2*foc2^2 + f6^2)^2 - ((f9 + f3*xc + f7*xc + f1*xc^2)^2 + foc1^2*(f3 + f1*xc)^2 + foc1^2*(f7 + f1*xc)^2 + foc2^2*(f6 + f4*xc)^2 + foc2^2*(f8 + f2*xc)^2 + f1^2*foc1^4 + f5^2*foc2^4 + f2^2*foc1^2*foc2^2 + f4^2*foc1^2*foc2^2)^2 + 4*(f1*(f7 + f1*xc)*foc1^3 + f2*(f8 + f2*xc)*foc1*foc2^2 + (f3 + f1*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc1)^2 + 4*(f4*(f7 + f1*xc)*foc1^2*foc2 + f5*(f8 + f2*xc)*foc2^3 + (f6 + f4*xc)*(f9 + f3*xc + f7*xc + f1*xc^2)*foc2)^2 + 4*foc1^2*foc2^2*(f3*f6 + f1*f4*xc^2 + f1*f6*xc + f3*f4*xc + f1*f4*foc1^2 + f2*f5*foc2^2)^2)];
r = roots(p');

    answ=processRoots_focalonly(r);
    if(answ>minans)
        solzarray(i,1)=answ;
    elseif(i>1)
        solzarray(i,1)=solzarray(i-1,1);
    else
        solzarray(i,1)=0;
    end
    xfarray(i,1)= xc;
end

end
function [sl]=processRoots_focalonly(p)

s=size(p,1);
minc=1000000000;
flagset=0;
sl=0;
for i=1:s
    if(real(p(i,1))>0 && abs( imag(p(i,1)))<minc)
        sl=real(p(i,1));
        minc=abs(imag(p(i,1)));
        flagset=1;
    end
end

if(flagset==0)
    % display('no solution');
end


end
%
% this is how you generate this formula
% syms f1 f2 f3 f4 f5 f6 f7 f8 f9 foc1 foc2 xc yc real
% K=[foc1 0 xc ; 0 foc2 yc; 0 0 1]
% F=[f1 f2 f3 ; f4 f5 f6 ; f7 f8 f9 ]
% G=(K')*F*K
% GGT=G*(G')
% v=(2*trace(GGT*GGT))-((trace(GGT))*(trace(GGT)))
% vs=simplify(v)
% R = collect(vs ,foc1) to collect terms for foc1

%2*(foc1^2*(F(3,1) + F(1,1)*xc + F(2,1)*yc)^2 + foc2^2*(F(3,2) + F(1,2)*xc
%+ F(2,2)*yc)^2 + (F(3,3) + F(1,3)*xc + F(3,1)*xc + F(2,3)*yc + F(3,2)*yc + F(1,1)*xc^2 + F(2,2)*yc^2 + F(1,2)*xc*yc + F(2,1)*xc*yc)^2)^2 + 4*(F(1,1)*(F(3,1) + F(1,1)*xc + F(2,1)*yc)*foc1^3 + F(1,2)*(F(3,2) + F(1,2)*xc + F(2,2)*yc)*foc1*foc2^2 + (F(1,3) + F(1,1)*xc + F(1,2)*yc)*(F(3,3) + F(1,3)*xc + F(3,1)*xc + F(2,3)*yc + F(3,2)*yc + F(1,1)*xc^2 + F(2,2)*yc^2 + F(1,2)*xc*yc + F(2,1)*xc*yc)*foc1)^2 + 4*(F(2,1)*(F(3,1) + F(1,1)*xc + F(2,1)*yc)*foc1^2*foc2 + F(2,2)*(F(3,2) + F(1,2)*xc + F(2,2)*yc)*foc2^3 + (F(2,3) + F(2,1)*xc + F(2,2)*yc)*(F(3,3) + F(1,3)*xc + F(3,1)*xc + F(2,3)*yc + F(3,2)*yc + F(1,1)*xc^2 + F(2,2)*yc^2 + F(1,2)*xc*yc + F(2,1)*xc*yc)*foc2)^2 - (foc1^2*(F(1,3) + F(1,1)*xc + F(1,2)*yc)^2 + foc1^2*(F(3,1) + F(1,1)*xc + F(2,1)*yc)^2 + foc2^2*(F(2,3) + F(2,1)*xc + F(2,2)*yc)^2 + foc2^2*(F(3,2) + F(1,2)*xc + F(2,2)*yc)^2 + (F(3,3) + F(1,3)*xc + F(3,1)*xc + F(2,3)*yc + F(3,2)*yc + F(1,1)*xc^2 + F(2,2)*yc^2 + F(1,2)*xc*yc + F(2,1)*xc*yc)^2 + F(1,1)^2*foc1^4 + F(2,2)^2*foc2^4 + F(1,2)^2*foc1^2*foc2^2 + F(2,1)^2*foc1^2*foc2^2)^2 + 2*foc1^4*(F(1,1)^2*foc1^2 + F(1,1)^2*xc^2 + 2*F(1,1)*F(1,2)*xc*yc + 2*F(1,1)*F(1,3)*xc + F(1,2)^2*foc2^2 + F(1,2)^2*yc^2 + 2*F(1,2)*F(1,3)*yc + F(1,3)^2)^2 + 2*foc2^4*(F(2,1)^2*foc1^2 + F(2,1)^2*xc^2 + 2*F(2,1)*F(2,2)*xc*yc + 2*F(2,1)*F(2,3)*xc + F(2,2)^2*foc2^2 + F(2,2)^2*yc^2 + 2*F(2,2)*F(2,3)*yc + F(2,3)^2)^2 + 4*foc1^2*foc2^2*(F(1,3)*F(2,3) + F(1,1)*F(2,1)*xc^2 + F(1,2)*F(2,2)*yc^2 + F(1,1)*F(2,3)*xc + F(1,3)*F(2,1)*xc + F(1,2)*F(2,3)*yc + F(1,3)*F(2,2)*yc + F(1,1)*F(2,1)*foc1^2 + F(1,2)*F(2,2)*foc2^2 + F(1,1)*F(2,2)*xc*yc + F(1,2)*F(2,1)*xc*yc)^2